//
// Created by 35702 on 2025/3/15.
//
#include "foc.h"
#include "main.h"

#include "math.h"
#include "tim.h"
#include "stdlib.h"
#include "stm32g4xx_it.h"
#include "stm32g4xx_hal_conf.h"
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))    // 约束函数--将范围确定下来
#define PI  3.1415926
//int pwmA = 32;
//int pwmB = 33;
//int pwmC = 25;



int8_t  txt[3];
int8_t  rx_data[3];
    // 步数 2次为一个



float voltage_power_supply=12.6;    // 电压电压的值
float shaft_angle=0,open_loop_timestamp=0;
float zero_electric_angle=0,Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0,dc_a=0,dc_b=0,dc_c=0;

uint32_t micros();


// 电角度求解   -  机械角度 - -- 极对数
float _electricalAngle(float shaft_angle, int pole_pairs) {
    return (shaft_angle * pole_pairs);
}

// 归一化角度到 [0,2PI]
float _normalizeAngle(float angle){
    float a = fmod(angle, 2*PI);   //取余运算可以用于归一化，列出特殊值例子算便知
    return a >= 0 ? a : (a + 2*PI);
}


// 设置PWM到控制器输出
void setPwm(float Ua, float Ub, float Uc) {

    // 计算占空比
    // 限制占空比从0到1
    dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
    dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
    dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );


    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1,dc_a*255);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2,dc_b*255);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,dc_c*255);
}

// 设置相电压
void setPhaseVoltage(float Uq,float Ud, float angle_el) {
    angle_el = _normalizeAngle(angle_el + zero_electric_angle);  // 角度归一化
    // 帕克逆变换
    Ualpha =  -Uq*sin(angle_el);
    Ubeta =   Uq*cos(angle_el);

    // 克拉克逆变换
    Ua = Ualpha + voltage_power_supply/2;
    Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;
    Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;
    setPwm(Ua,Ub,Uc);
}


//开环速度函数
float velocityOpenloop(float target_velocity){
    unsigned long now_us = micros();  //获取从开启芯片以来的微秒数，它的精度是 4 微秒。 micros() 返回的是一个无符号长整型（unsigned long）的值

    //计算当前每个Loop的运行时间间隔
    float Ts = (now_us - open_loop_timestamp) * 1e-6f;


    if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;


// 轴角度 - 限幅
    shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);

    float Uq = voltage_power_supply/3;   // 不能超过最大值

    setPhaseVoltage(Uq,  0, _electricalAngle(shaft_angle, 7));

    open_loop_timestamp = now_us;  //用于计算下一个时间间隔

    return Uq;
}


void loop() {
while(1)
{


	velocityOpenloop(10);

}
}



__STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
{
    return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
}
static uint32_t getCurrentMicros(void)
{
    /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
    GXT_SYSTICK_IsActiveCounterFlag();
    uint32_t m = HAL_GetTick();
    const uint32_t tms = SysTick->LOAD + 1;
    __IO uint32_t u = tms - SysTick->VAL;
    if (GXT_SYSTICK_IsActiveCounterFlag()) {
        m = HAL_GetTick();
        u = tms - SysTick->VAL;
    }
    return (m * 1000 + (u * 1000) / tms);
}
//获取系统时间，单位us
uint32_t micros(void)
{
    return getCurrentMicros();
}





//
//void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
//{
//    if (huart == &huart1)
//    {
//
//        HAL_UART_Receive_IT(&huart1, rx_data, 3);  // 再次开启接收中断
//        HAL_UART_Transmit_IT(&huart1, rx_data, 3);
//    }
//}
//
//
//// 5ms发送中断
//void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
//{
//
//    if (htim == &htim2)
//    {
//
//        data_get_fff =  adc_value /4095.0 * 100;
//
//        sprintf(txt,"<data1>:%.2f\n", data_get_fff );
//
//
//        HAL_UART_Transmit(&huart1, txt, sizeof(txt), 100);
//    }
//}
//
//
